lear; clc; close all;
% 线性系统系数矩阵
A=[0.1 0.12; 0 1]; B=[1 ; 0.25];
% A=[1 1; 0 1]; B=[0.2 1; 0.5 2];
% 初始状态量-如果不能在下一步回到约束范围内，则会造成无解
x0=[0; 0];
% x0=[0 0 ;0 0];

% 预测步长
Np=5;
% 优化目标参数，加权矩阵
Q=1*eye(2); R=1;
% Q=1*eye(2); R=[1 0 ;0 .1];
% 转化为用控制量ut表示的，关于状态量的推导方程的矩阵
At=[]; Bt=[]; temp=[];
% 转换后的加权矩阵
Qt=[]; Rt=[];

% 加权矩阵的计算过程，以及推导方程矩阵的叠加过程
for i=1:Np
        At=[At; A^i];
        Bt=[Bt zeros(size(Bt,1), size(B,2));
            A^(i-1)*B temp];
        temp=[A^(i-1)*B temp];
        
        Qt=[Qt zeros(size(Qt,1),size(Q,1));
            zeros(size(Q,1),size(Qt,1)) Q];
        Rt=[Rt zeros(size(Rt,1),size(R,1));
            zeros(size(R,1),size(Rt,1)) R];
end
% 参考输入
% xd=10*ones(1, 2*Np);
xd=[1 ; 1];
% xd=[1 1;1 1];
% 控制量ut的上下限
lb=-1*ones(Np,1);
ub=1*ones(Np,1);
% 控制量ut的初始值
% u0=zeros(Np,1);
u0=zeros(Np,0);

% 转换后的优化目标函数矩阵(代价函数)，循环优化函数中H后的表达式为优化目标的另一项
H=2*(Bt'*Qt*Bt + Rt);
% 转换后的优化中的不等式约束左边系数矩阵，后面循环中的bi为不等式右边
% Ai=[Bt; -Bt];
% 声明u来保存每一步采用的控制量
u=[];
% 误差反馈
xe=x0-xd;
x=xe; %用来计算和保存误差状态量
xxe=xe; % 用来保存误差值
xx=x0; % 用来显示最后的真实状态量
% figure();

for k=1:20
    % 关于ut的不等式约束，实际上约束的是状态量，常数4就是状态量约束的上下边界
%     bi=[5-At*xe; 5+At*xe];
    % quadprog 解二次规划问题
    [ut, fval, exitflag]=quadprog(H,(2*xe'*At'*Qt*Bt)',[],[],[],[],lb,ub,u0);
    % 显示求解结果是否正常
    if exitflag == 1
        fprintf("正常");
    elseif exitflag == -3
        fprintf("目标函数无解");
    elseif exitfilag == 0
        fprintf("迭代步数超过最大值")
    end
    fprintf('%d\n', exitflag)
%     xt = At*xk + Bt*ut;
%     
%     plot(xt');
%     hold on;
    xp=[]; %xp用来记录每个time step上对未来的预测状态量
    xp_0=xe; %第一个值为当前状态量
    xp=xp_0;
    for j=1:Np
        xp(:, j+1) = A*xp(:, j) + B*ut(j);

    end
    for i =1:Np
        fprintf('ut(%d)=%.2f-->\n',i,ut(i));
        fprintf('B*ut = %.2f\n',B*ut(i));
        fprintf("==================\n")
    end

% 对每一次预测的状态量作图
    tp=k:(k+Np);
    tpu=k:(k+Np-1);
    subplot(2, 1, 1);
    plot(tp,xp','linewidth',1.5);
    hold on;
    subplot(2, 1, 2);
    plot(tpu, ut, 'linewidth', 1.5);
    hold on;
    
    % 采用优化得到的控制量的第一个元素作为实际作用的控制量，代入到原系统中得到下一个时刻的状态量
    u(k) = ut(1);
    x(:, k+1) = A*x(:, k) + B*u(k);
    xe = x(:, k+1);
    xxe=[xxe, xe];
    xx=[xx, xe+xd];
    % 对优化初始值进行修改，采用预测值的后段作为下一步的初始值
    u0 = [ut(2:Np); ut(Np)];
end

% 计算x状态值的绝对值之和，作为衡量控制效果的指标
count=sum(abs(xxe'));

% figure();
% plot(xxe);
% hold on;

figure();
subplot(2,1,1);
plot(xx','linewidth',1.5); grid on;
ref = refline(0,1);
ref.Color = 'm';
ylabel('V');
legend('V','I','参考值'); title('states');

subplot(2,1,2);
plot(u,'linewidth',1.5); grid on;
legend('氢气压强'); title('inputs');
ylabel('P_{H2}');


